#include "AS5047P.h"


int Parity_Bit_Calculate(u16 data)
{
    int parity_bit_value = 0;
    data &= AS5047P_DATA_MASK;
    while(data != 0)
    {
        parity_bit_value ^= data;
        data >>= 1;
    }
    return (parity_bit_value & 0x01);
}
int16_t AS5047P_ReadREG(SPI_Device device, uint16_t addr)
{
    uint16_t Timeout = 0x1FFF;
    uint32_t temp;
    if(Parity_Bit_Calculate(addr))
    {
        addr &= ~AS5047P_EVEN_PARITY_MASK;  /*Even          */
    }
    else
    {
        addr  |= AS5047P_EVEN_PARITY_MASK;  /*Odd           */
    }
    addr |= AS5047P_RW_MASK;                /*Read Register */
    while(!WaitSPI_Free(device) && Timeout--);
    _SPI_SendData(device, addr);
    while(!WaitSPI_Free(device) && Timeout--);
    _SPI_SendData(device, AS5047P_NOP_REG);

    temp = _SPI_RecieveData(device);
    if((temp & AS5047P_EVEN_PARITY_MASK) == (Parity_Bit_Calculate(temp) & AS5047P_EVEN_PARITY_MASK))
    {
        return temp & AS5047P_DATA_MASK;
    }
    else
    {
        return AS5047P_PARITY_ERROR;
    }
}

int16_t AS5047P_WriteREG(SPI_Device device, uint16_t addr, uint16_t data)
{
    uint16_t Timeout = 0x1FFF;
    if(Parity_Bit_Calculate(addr))
    {
        addr &= ~AS5047P_EVEN_PARITY_MASK;  /*Even          */
    }
    else
    {
        addr  |= AS5047P_EVEN_PARITY_MASK;  /*Odd           */
    }

    if(Parity_Bit_Calculate(data))
    {
        data &= ~AS5047P_EVEN_PARITY_MASK;  /*Even          */
    }
    else
    {
        data  |= AS5047P_EVEN_PARITY_MASK;  /*Odd           */
    }
    addr &= ~AS5047P_RW_MASK;               /*Reset [14] to specify write mode*/
    data &= ~AS5047P_RW_MASK;               /*Always 0 when write reg*/
    while(!WaitSPI_Free(device) && Timeout--);
    _SPI_SendData(device, addr);
    while(!WaitSPI_Free(device) && Timeout--);
    _SPI_SendData(device, data);
    while(!WaitSPI_Free(device) && Timeout--);
    if(Timeout)
    {
        return AS5047P_WRITE_OK;
    }
    else
    {
        return AS5047P_RW_TIMEOUT;
    }
}
